<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">

<!-- 
	Copyright (C) 2007, 2008, 2009, 2010, 2011. PARP Research Group.
	<http://perception.inf.um.es>
	University of Murcia, Spain.

	This file is part of the QVision library.

	QVision is free software: you can redistribute it and/or modify
	it under the terms of the GNU Lesser General Public License as
	published by the Free Software Foundation, version 3 of the License.

	QVision is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public
	License along with QVision. If not, see <http://www.gnu.org/licenses/>.
-->

<html><head><meta http-equiv="content-Type" content="text/html;charset=UTF-8">
<title>QVision: Qt&#39;s Image, Video and Computer Vision Library</title>
<meta name="title" content="QVision" />
<meta name="dc.title" content="QVision" />
<meta name="url" content="http://perception.inf.um.es/QVision" />
<meta name="author" content="PARP Research Group - http://perception.inf.um.es" />
<meta name="revisit-after" content="30 DAYS"/>
<meta name="robots" content="index,follow"/>
<meta name="classification" content="*">
<meta name="rating" content="Safe For Kids">
<meta name="distribution" content="GLOBAL"/>
<meta name="description" content="Qt's Image, Video and Computer Vision Library"/>
<meta name="page-topic" content="Computer Vision research and prototype programming"/>
<meta name="geo.country" content="ES" />

<!--
Keywords:
By license:		open source, gnu, lgpl, gpl, free
By theme:		computer vision, image processing, robotics, programming, source, development
By usage:		library, toolkit, framework, prototype, application
By programming specs:	object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping
Interoperability with:	Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack
Functionallity:		image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface
Main data-types:	matrix, vector, tensor, quaternion, image, polyline
Video sources:		webcam, camera, stream
Devices:		embedded, desktop computer, laptop, mini-laptop
Authors:		PARP research group. University of Murcia, Spain.
-->

<meta name="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="pragma" content="no-cache"/>
<meta http-equiv="title" content="QVision"/>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
<link href="tabs.css" rel="stylesheet" type="text/css" />
<link rel="shortcut icon" href="favicon.ico" />
</head><body>

<table width="100%"><tr>
	<td><a href="http://perception.inf.um.es/"><img src="parp.png" border="0" /> <big>PARP Research Group</big></a></td>
	<td align="right"><a href="http://www.um.es/"><big>Universidad de Murcia</big> <img src="um.png" border="0" /></a></td>
</tr></table>

<hr /><br />

<table width="95%" align="center"><tr><td>

<!-- Generated by Doxygen 1.6.3 -->
<script type="text/javascript"><!--
var searchBox = new SearchBox("searchBox", "search",false,'Search');
--></script>
<div class="navigation" id="top">
  <div class="tabs">
    <ul>
      <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
      <li><a href="pages.html"><span>Related&nbsp;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
          <form id="FSearchBox" action="search.php" method="get">
            <img id="MSearchSelect" src="search/search.png" alt=""/>
            <input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S" 
                   onfocus="searchBox.OnSearchFieldFocus(true)" 
                   onblur="searchBox.OnSearchFieldFocus(false)"/>
          </form>
        </div>
      </li>
    </ul>
  </div>
  <div class="tabs">
    <ul>
      <li><a href="files.html"><span>File&nbsp;List</span></a></li>
      <li><a href="globals.html"><span>File&nbsp;Members</span></a></li>
    </ul>
  </div>
<h1>examples/OpenCV/kalmanExample/kalmanExample.cpp</h1><a href="kalmanExample_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/*</span>
<a name="l00002"></a>00002 <span class="comment"> *      Copyright (C) 2009, 2010, 2011, 2012. PARP Research Group.</span>
<a name="l00003"></a>00003 <span class="comment"> *      &lt;http://perception.inf.um.es&gt;</span>
<a name="l00004"></a>00004 <span class="comment"> *      University of Murcia, Spain.</span>
<a name="l00005"></a>00005 <span class="comment"> *</span>
<a name="l00006"></a>00006 <span class="comment"> *      This file is part of the QVision library.</span>
<a name="l00007"></a>00007 <span class="comment"> *</span>
<a name="l00008"></a>00008 <span class="comment"> *      QVision is free software: you can redistribute it and/or modify</span>
<a name="l00009"></a>00009 <span class="comment"> *      it under the terms of the GNU Lesser General Public License as</span>
<a name="l00010"></a>00010 <span class="comment"> *      published by the Free Software Foundation, version 3 of the License.</span>
<a name="l00011"></a>00011 <span class="comment"> *</span>
<a name="l00012"></a>00012 <span class="comment"> *      QVision is distributed in the hope that it will be useful,</span>
<a name="l00013"></a>00013 <span class="comment"> *      but WITHOUT ANY WARRANTY; without even the implied warranty of</span>
<a name="l00014"></a>00014 <span class="comment"> *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the</span>
<a name="l00015"></a>00015 <span class="comment"> *      GNU Lesser General Public License for more details.</span>
<a name="l00016"></a>00016 <span class="comment"> *</span>
<a name="l00017"></a>00017 <span class="comment"> *      You should have received a copy of the GNU Lesser General Public</span>
<a name="l00018"></a>00018 <span class="comment"> *      License along with QVision. If not, see &lt;http://www.gnu.org/licenses/&gt;.</span>
<a name="l00019"></a>00019 <span class="comment"> */</span>
<a name="l00020"></a>00020 
<a name="l00039"></a>00039 <span class="preprocessor">#include &lt;stdio.h&gt;</span>
<a name="l00040"></a>00040 <span class="preprocessor">#include &lt;stdlib.h&gt;</span>
<a name="l00041"></a>00041 <span class="preprocessor">#include &lt;iostream&gt;</span>
<a name="l00042"></a>00042 
<a name="l00043"></a>00043 <span class="preprocessor">#include &lt;QDebug&gt;</span>
<a name="l00044"></a>00044 <span class="preprocessor">#include &lt;QVApplication&gt;</span>
<a name="l00045"></a>00045 <span class="preprocessor">#include &lt;QVDefaultGUI&gt;</span>
<a name="l00046"></a>00046 
<a name="l00047"></a>00047 <span class="preprocessor">#include &lt;QVNumericPlot&gt;</span>
<a name="l00048"></a>00048 <span class="preprocessor">#include &lt;QVVector&gt;</span>
<a name="l00049"></a>00049 <span class="preprocessor">#include &lt;QVMatrix&gt;</span>
<a name="l00050"></a>00050 
<a name="l00051"></a>00051 <span class="preprocessor">#include &lt;qvmath.h&gt;</span>
<a name="l00052"></a>00052 
<a name="l00053"></a>00053 <span class="preprocessor">#ifndef DOXYGEN_IGNORE_THIS</span>
<a name="l00054"></a>00054 <span class="preprocessor"></span>
<a name="l00055"></a>00055 <span class="preprocessor">#define MEASUREMENT_NOISE_COV   0.5</span>
<a name="l00056"></a>00056 <span class="preprocessor"></span><span class="preprocessor">#define PROCESS_NOISE_COV       0.1</span>
<a name="l00057"></a>00057 <span class="preprocessor"></span>
<a name="l00058"></a>00058 <span class="keyword">class </span>QVKalmanFilter
<a name="l00059"></a>00059         {
<a name="l00060"></a>00060         <span class="keyword">private</span>:
<a name="l00061"></a>00061                 CvKalman* kalman;
<a name="l00062"></a>00062 
<a name="l00063"></a>00063         <span class="keyword">public</span>:
<a name="l00064"></a>00064                 QVKalmanFilter( <span class="keyword">const</span> <span class="keywordtype">int</span> dynamParams, <span class="keyword">const</span> <span class="keywordtype">int</span> measureParams, <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;transitionMatrix,
<a name="l00065"></a>00065                                 <span class="keyword">const</span> <span class="keywordtype">double</span> processNoiseCovariance, <span class="keyword">const</span> <span class="keywordtype">double</span> measurementNoiseCovariance):
<a name="l00066"></a>00066                         kalman(cvCreateKalman(dynamParams, measureParams, 0))
<a name="l00067"></a>00067                         {
<a name="l00068"></a>00068                         CvMat *transitionCvMatrix = transitionMatrix.<a class="code" href="classQVMatrix.html#a129ef6e4e7854e7faa11234ac6686239" title="Converts the matrix to an OpenCV matrix structure.">toCvMat</a>(CV_32F);
<a name="l00069"></a>00069                         cvCopy(transitionCvMatrix, kalman-&gt;transition_matrix);
<a name="l00070"></a>00070 
<a name="l00071"></a>00071                         cvReleaseMat(&amp;transitionCvMatrix);
<a name="l00072"></a>00072                         
<a name="l00073"></a>00073                         cvSetIdentity(kalman-&gt;process_noise_cov,        cvRealScalar(processNoiseCovariance));
<a name="l00074"></a>00074                         cvSetIdentity(kalman-&gt;measurement_noise_cov,    cvRealScalar(measurementNoiseCovariance));
<a name="l00075"></a>00075                         cvSetIdentity(kalman-&gt;measurement_matrix,       cvRealScalar(1));
<a name="l00076"></a>00076                         cvSetIdentity(kalman-&gt;error_cov_post,           cvRealScalar(1));
<a name="l00077"></a>00077 
<a name="l00078"></a>00078                         cvZero( kalman-&gt;state_post );
<a name="l00079"></a>00079                         }
<a name="l00080"></a>00080 
<a name="l00081"></a>00081                 <span class="keyword">const</span> <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> predict()<span class="keyword"> const</span>
<a name="l00082"></a>00082 <span class="keyword">                        </span>{
<a name="l00083"></a>00083                         <span class="keywordflow">return</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a>(cvKalmanPredict( kalman, 0 ));
<a name="l00084"></a>00084                         }
<a name="l00085"></a>00085 
<a name="l00086"></a>00086                 <span class="keywordtype">void</span> correct(<span class="keyword">const</span> <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> &amp;z)
<a name="l00087"></a>00087                         {
<a name="l00088"></a>00088                         CvMat* z_k = z.<a class="code" href="classQVVector.html#a57387ffc7c1b3ee7de38adb5155d3707" title="Returns this vector in the form of a column matrix.">toColumnMatrix</a>().<a class="code" href="classQVMatrix.html#a129ef6e4e7854e7faa11234ac6686239" title="Converts the matrix to an OpenCV matrix structure.">toCvMat</a>(CV_32F);
<a name="l00089"></a>00089                         cvKalmanCorrect( kalman, z_k );
<a name="l00090"></a>00090                         cvReleaseMat(&amp;z_k);
<a name="l00091"></a>00091                         }
<a name="l00092"></a>00092 
<a name="l00093"></a>00093         <span class="keyword">static</span> <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> linearMotionTransitionMatrix(<span class="keyword">const</span> <span class="keywordtype">int</span> size = 1, <span class="keyword">const</span> <span class="keywordtype">double</span> step = 0.1)
<a name="l00094"></a>00094                 {
<a name="l00095"></a>00095                 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> transitionMatrix = <a class="code" href="classQVMatrix.html#a6eb15980d17c17d4a55ab09a77bcd715" title="Creates an identity matrix.">QVMatrix::identity</a>(2*size);
<a name="l00096"></a>00096                 <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; size; i++)
<a name="l00097"></a>00097                         transitionMatrix(i,size+i) = step;
<a name="l00098"></a>00098         
<a name="l00099"></a>00099                 <span class="keywordflow">return</span> transitionMatrix;
<a name="l00100"></a>00100                 }
<a name="l00101"></a>00101         };
<a name="l00102"></a>00102 
<a name="l00103"></a>00103 
<a name="l00104"></a>00104 <span class="keyword">class </span>KalmanFilterBlock: <span class="keyword">public</span> <a class="code" href="classQVProcessingBlock.html" title="Base class for Block threads.">QVProcessingBlock</a>
<a name="l00105"></a>00105         {
<a name="l00106"></a>00106         <span class="keyword">private</span>:
<a name="l00107"></a>00107                 QVKalmanFilter kalmanFilter;
<a name="l00108"></a>00108         
<a name="l00109"></a>00109         <span class="keyword">public</span>:
<a name="l00110"></a>00110                 KalmanFilterBlock(<span class="keyword">const</span> QString name): <a class="code" href="classQVProcessingBlock.html" title="Base class for Block threads.">QVProcessingBlock</a>(name),
<a name="l00111"></a>00111                         kalmanFilter(2, 1, QVKalmanFilter::linearMotionTransitionMatrix(), PROCESS_NOISE_COV, MEASUREMENT_NOISE_COV)
<a name="l00112"></a>00112                         {
<a name="l00113"></a>00113                         addProperty&lt;double&gt;(<span class="stringliteral">&quot;Kalman[sin(x) + noise]&quot;</span>, outputFlag);
<a name="l00114"></a>00114                         addProperty&lt;double&gt;(<span class="stringliteral">&quot;sin(x)&quot;</span>, outputFlag);
<a name="l00115"></a>00115                         addProperty&lt;double&gt;(<span class="stringliteral">&quot;sin(x) + noise&quot;</span>, outputFlag);
<a name="l00116"></a>00116 
<a name="l00117"></a>00117                         addProperty&lt;double&gt;(<span class="stringliteral">&quot;Kalman error&quot;</span>, outputFlag);
<a name="l00118"></a>00118                         addProperty&lt;double&gt;(<span class="stringliteral">&quot;Measurement error&quot;</span>, outputFlag);
<a name="l00119"></a>00119                         }
<a name="l00120"></a>00120 
<a name="l00121"></a>00121                 <span class="keywordtype">void</span> <a class="code" href="classQVProcessingBlock.html#a0283c799776e0c938538de0b0ae6b463" title="Function to be defined in subclasses with processing code.">iterate</a>()
<a name="l00122"></a>00122                         {
<a name="l00123"></a>00123                         <span class="comment">// Read sensor, actualize real state.</span>
<a name="l00124"></a>00124                         <span class="keyword">const</span> <span class="keywordtype">double</span>    realState = 1 - cos((<span class="keywordtype">double</span>) <a class="code" href="classQVProcessingBlock.html#a95c56bae078a84bf689b7ee8073bcf7c" title="Function to obtain the number of iteration the block has performed.">getIteration</a>() / 512.0),
<a name="l00125"></a>00125                                         measurementNoise = ((double)(rand()%2000)/1000.0 - 1.0) * sqrt(MEASUREMENT_NOISE_COV),
<a name="l00126"></a>00126                                         processNoise = ((double)(rand()%2000)/1000.0 - 1.0) * sqrt(PROCESS_NOISE_COV);
<a name="l00127"></a>00127 
<a name="l00128"></a>00128                         <span class="comment">// Predict and correct Kalman filter state</span>
<a name="l00129"></a>00129                         <span class="keyword">const</span> <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> y = kalmanFilter.predict();
<a name="l00130"></a>00130                         kalmanFilter.correct(<a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a>(1, realState + processNoise + measurementNoise));
<a name="l00131"></a>00131 
<a name="l00132"></a>00132                         <span class="comment">// Publish results</span>
<a name="l00133"></a>00133                         setPropertyValue&lt;double&gt;(<span class="stringliteral">&quot;Kalman[sin(x) + noise]&quot;</span>, y[0]);
<a name="l00134"></a>00134                         setPropertyValue&lt;double&gt;(<span class="stringliteral">&quot;sin(x)&quot;</span>, realState + processNoise);
<a name="l00135"></a>00135                         setPropertyValue&lt;double&gt;(<span class="stringliteral">&quot;sin(x) + noise&quot;</span>, realState + processNoise + measurementNoise);
<a name="l00136"></a>00136                         setPropertyValue&lt;double&gt;(<span class="stringliteral">&quot;Kalman error&quot;</span>, ABS(realState - y[0]));
<a name="l00137"></a>00137                         setPropertyValue&lt;double&gt;(<span class="stringliteral">&quot;Measurement error&quot;</span>, ABS(processNoise + measurementNoise));
<a name="l00138"></a>00138                         }
<a name="l00139"></a>00139         };
<a name="l00140"></a>00140 
<a name="l00141"></a>00141 <span class="preprocessor">#include &lt;QVYUV4MPEG2WriterBlock&gt;</span>
<a name="l00142"></a>00142 <span class="preprocessor">#include &lt;QVNumericPlot&gt;</span>
<a name="l00143"></a>00143 
<a name="l00144"></a>00144 <span class="keywordtype">int</span> main(<span class="keywordtype">int</span> argc, <span class="keywordtype">char</span> *argv[])
<a name="l00145"></a>00145         {
<a name="l00146"></a>00146         <a class="code" href="classQVApplication.html" title="Main class for QVision applications.">QVApplication</a> app(argc, argv,
<a name="l00147"></a>00147                 <span class="stringliteral">&quot;Example program for QVision library. Applies Kalman filtering to an input function.&quot;</span>
<a name="l00148"></a>00148                 );
<a name="l00149"></a>00149 
<a name="l00150"></a>00150         KalmanFilterBlock kalmanFilterBlock(<span class="stringliteral">&quot;Corners Block&quot;</span>);
<a name="l00151"></a>00151 
<a name="l00152"></a>00152         <a class="code" href="classQVDefaultGUI.html" title="Main user interface widget for QVision applications.">QVDefaultGUI</a> gui;
<a name="l00153"></a>00153 
<a name="l00154"></a>00154         <a class="code" href="classQVNumericPlot.html" title="Class for plot graphs of block&amp;#39;s int or double properties.">QVNumericPlot</a> measurePlot(<span class="stringliteral">&quot;Obtained values&quot;</span>);
<a name="l00155"></a>00155         kalmanFilterBlock.linkProperty(<span class="stringliteral">&quot;Kalman[sin(x) + noise]&quot;</span>, measurePlot);
<a name="l00156"></a>00156         kalmanFilterBlock.linkProperty(<span class="stringliteral">&quot;sin(x)&quot;</span>, measurePlot);
<a name="l00157"></a>00157         kalmanFilterBlock.linkProperty(<span class="stringliteral">&quot;sin(x) + noise&quot;</span>, measurePlot);
<a name="l00158"></a>00158 
<a name="l00159"></a>00159         <a class="code" href="classQVNumericPlot.html" title="Class for plot graphs of block&amp;#39;s int or double properties.">QVNumericPlot</a> errorPlot(<span class="stringliteral">&quot;Estimation errors&quot;</span>);
<a name="l00160"></a>00160         kalmanFilterBlock.linkProperty(<span class="stringliteral">&quot;Kalman error&quot;</span>, errorPlot);
<a name="l00161"></a>00161         kalmanFilterBlock.linkProperty(<span class="stringliteral">&quot;Measurement error&quot;</span>, errorPlot);
<a name="l00162"></a>00162 
<a name="l00163"></a>00163         <span class="keywordflow">return</span> app.exec();
<a name="l00164"></a>00164 
<a name="l00165"></a>00165         }
<a name="l00166"></a>00166 
<a name="l00167"></a>00167 <span class="preprocessor">#endif</span>
<a name="l00168"></a>00168 <span class="preprocessor"></span>
</pre></div></div>
</td></tr></table>

<br /><hr><br />
<center><a href="http://perception.inf.um.es/QVision">QVision framework</a>.
<a href="http://perception.inf.um.es">PARP research group</a>.
Copyright &copy; 2007, 2008, 2009, 2010, 2011.</center>
<br />
</body>
</html>
